#include "ses_timer.h"
#include "pscheduler.h"
#include "ses_uart.h"

// Timer registers
#define TIMER2_INT_vect  			TIMER2_COMPA_vect
#define TIMER2_MODE_A				1<< WGM21 	// | (1<< CMP...) alternatively
#define TIMER2_MODE_B				0x04
#define TIMER2_INT_MASK				1<<OCIE2A
#define TIMER2_INT_FLAG         				// TODO check if needed
#define TIMER2_CYC_FOR_1MILLISEC	250 		// triggers interrupt after 1 ms
/* Saves stack of current task. */
#define SAVE() \
	asm volatile( \
		"push  r0             \n\t" \
		"push  r1             \n\t" \
		"in    r0, __SREG__   \n\t" \
		"push  r0             \n\t" \
		"push  r1             \n\t" \
		"clr   r1             \n\t" \
		"push  r2             \n\t" \
		"push  r3             \n\t" \
		"push  r4             \n\t" \
		"push  r5             \n\t" \
		"push  r6             \n\t" \
		"push  r7             \n\t" \
		"push  r8             \n\t" \
		"push  r9             \n\t" \
		"push  r10            \n\t" \
		"push  r11            \n\t" \
		"push  r12            \n\t" \
		"push  r13            \n\t" \
		"push  r14            \n\t" \
		"push  r15            \n\t" \
		"push  r16            \n\t" \
		"push  r17            \n\t" \
		"push  r18            \n\t" \
		"push  r19            \n\t" \
		"push  r20            \n\t" \
		"push  r21            \n\t" \
		"push  r22            \n\t" \
		"push  r23            \n\t" \
		"push  r24            \n\t" \
		"push  r25            \n\t" \
		"push  r26            \n\t" \
		"push  r27            \n\t" \
		"push  r28            \n\t" \
		"push  r29            \n\t" \
		"push  r30            \n\t" \
		"push  r31            \n\t" \
		"in    r0,   __SP_L__ \n\t" \
		"in    r1,   __SP_H__ \n\t" \
		"st    %a0+, r0	      \n\t" \
		"st    %a0,  r1	      \n\t" \
		: \
		: "e" (&pstack) \
		: "memory" \
	);
/* Restores stack from current task. */
#define RESTORE() \
	asm volatile ( \
		"lds   r26,      pstack     \n\t" \
		"lds   r27,      pstack + 1 \n\t" \
		"out   __SP_L__, r26        \n\t" \
		"out   __SP_H__, r27        \n\t" \
		"pop   r31                  \n\t" \
		"pop   r30                  \n\t" \
		"pop   r29                  \n\t" \
		"pop   r28                  \n\t" \
		"pop   r27                  \n\t" \
		"pop   r26                  \n\t" \
		"pop   r25                  \n\t" \
		"pop   r24                  \n\t" \
		"pop   r23                  \n\t" \
		"pop   r22                  \n\t" \
		"pop   r21                  \n\t" \
		"pop   r20                  \n\t" \
		"pop   r19                  \n\t" \
		"pop   r18                  \n\t" \
		"pop   r17                  \n\t" \
		"pop   r16                  \n\t" \
		"pop   r15                  \n\t" \
		"pop   r14                  \n\t" \
		"pop   r13                  \n\t" \
		"pop   r12                  \n\t" \
		"pop   r11                  \n\t" \
		"pop   r10                  \n\t" \
		"pop   r9                   \n\t" \
		"pop   r8                   \n\t" \
		"pop   r7                   \n\t" \
		"pop   r6                   \n\t" \
		"pop   r5                   \n\t" \
		"pop   r4                   \n\t" \
		"pop   r3                   \n\t" \
		"pop   r2                   \n\t" \
		"pop   r1                   \n\t" \
		"pop   r0                   \n\t" \
		"out   __SREG__, r0         \n\t" \
		"pop   r0                   \n\t" \
	);

/* Callback function is executed on interrupt. */
pTimerCallback callback;



void timer2_init(pTimerCallback cb) {

	TCNT2 = 0;
	TCCR2A = TIMER2_MODE_A;
	TCCR2B = TIMER2_MODE_B;
	OCR2A = TIMER2_CYC_FOR_1MILLISEC;
	TIMSK2 = TIMER2_INT_MASK;

	// The callback function will point to the function cb. Each time an
	// interrupt is triggered callback() will be executed.
	callback = cb;
}

/*
 * Triggered every 1 ms.
 */
ISR(TIMER2_INT_vect, ISR_NAKED) {

	pstack = task->pstack;	// get stack pointer of current task

	SAVE();					// save stack of current task

	callback();				// execute task

	task = task->next;		// go to next task
	pstack = task->pstack;	// get stack pointer of next task

	RESTORE();				// restore next task

	asm volatile(
			"reti \n\t"
	);

}
